﻿/*
 * ©2009-2012 XELF
 * http://xelf.info
 */

#if XNA || SILVERLIGHT || WINDOWS_PHONE
using Microsoft.Xna.Framework;
using _MathHelper = Microsoft.Xna.Framework.MathHelper;
#else
using _MathHelper = XELF.Framework.MathHelper;
#endif

namespace XELF.Framework {

	public partial struct DualQuaternion {
		/// <summary>
		/// SCrew Linear Interpolation
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="amount"></param>
		/// <param name="result"></param>
		public static void Sclerp(ref DualQuaternion a, ref DualQuaternion b, float amount, out DualQuaternion result) {
			float side;
			Quaternion.Dot(ref a.Real, ref b.Real, out side);
			DualQuaternion _b = (side >= 0) ? b : -b;

			DualQuaternion i, d;
			DualQuaternion.Invert(ref a, out i);
			DualQuaternion.Multiply(ref i, ref _b, out d);

			var cosine = _MathHelper.Clamp(d.Real.W, -1, +1);
			var theta = MathHelper.Acos(cosine) * amount;
			var s = MathHelper.Sin(theta);
			var c = MathHelper.Cos(theta);

			var axis = new Vector3(d.Real.X, d.Real.Y, d.Real.Z);
			var l = axis.LengthSquared();
			var r = l < 1e-5f ? 1.0f : MathHelper.Rsqrt(l);
			Quaternion dd;
			var rs = r * s;
			Quaternion.Multiply(ref d.Dual, rs, out dd);

			Vector3.Multiply(ref axis, r, out axis);

			var u = (d.Real.W * dd.W - d.Dual.W * amount * c) * r;

			d = new DualQuaternion(
				axis.X * s,
				axis.Y * s,
				axis.Z * s,
				c,
				axis.X * u + dd.X,
				axis.Y * u + dd.Y,
				axis.Z * u + dd.Z,
				dd.W * amount);

			DualQuaternion.Multiply(ref a, ref d, out result);
		}

	}

}
